{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Direct and inverse geometry of 3d robots\n",
    "This notebook introduces the kinematic tree of Pinocchio for a serial manipulator, explain how to compute the forward and inverse geometry (from configuration to end-effector placements, and inversely). The ideas are examplified with a simplified case-study taken from parallel robotics.\n",
    "\n",
    "本笔记本介绍了用于串联机械手的Pinocchio运动学树，解释了如何计算正向和反向几何（从配置到末端执行器的位姿，反之亦然）。这些概念通过一个取自并联机器人的简化案例研究进行说明。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "NB: as for all the tutorials, a magic command %do_not_load is introduced to hide the solutions to some questions. Change it for %load if you want to see (and execute) the solution.\n"
     ]
    }
   ],
   "source": [
    "import magic_donotload  # noqa: F401"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Set up\n",
    "We will need Pinocchio, Gepetto-Viewer, SciPy for the solvers"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "import time\n",
    "import math\n",
    "import numpy as np\n",
    "from numpy.linalg import norm\n",
    "import pinocchio as pin\n",
    "import example_robot_data as robex\n",
    "from scipy.optimize import fmin_bfgs"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Kinematic tree in Pinocchio\n",
    "Let's now play with 3D robots. We will load the models from URDF files.\n",
    "\n",
    "*The robot UR5* is a low-cost manipulator robot with good performances. It is a fixed robot with one 6-DOF arms developed by the Danish company Universal Robot. All its 6 joints are revolute joints. Its configuration is in R^6 and is not subject to any constraint. The model of UR5 is described in a URDF file, with the visuals of the bodies of the robot being described as meshed (i.e. polygon soups) using the Collada format \".dae\". Both the URDF and the DAE files are available in the repository in the model directory. \n",
    "\n",
    "This robot model, as well as other models used in the notebooks, are installed from the apt paquet robotpkg-example-robot-data and stored in /opt/openrobots/share/example-robot-data.\n",
    "\n",
    "## Pinocchio中的运动学树\n",
    "现在让我们来研究3D机器人。我们将从URDF文件加载模型。\n",
    "\n",
    "“UR5机器人”是一款性价比高且性能良好的操纵机器人。它是由丹麦公司优傲机器人（Universal Robot）开发的具有一个6自由度机械臂的固定机器人。其所有6个关节均为旋转关节。它的位形空间为R^6 ，不受任何约束。UR5的模型在一个URDF文件中描述，机器人机身的视觉效果使用Collada格式“.dae”以网格（即多边形集合）的形式描述。URDF和DAE文件都可以在存储库的模型目录中找到。\n",
    "\n",
    "这个机器人模型以及本笔记本中使用的其他模型，通过软件包robotpkg-example-robot-data安装，并存储在/opt/openrobots/share/example-robot-data中。 \n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot = robex.load('ur5')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The kinematic tree is represented by two C++ objects called Model (which contains the model constants: lengths, masses, names, etc) and Data (which contains the working memory used by the model algorithms). Both C\\++ objects are contained in a unique Python class. The first class is called RobotWrapper and is generic.\n",
    "\n",
    "运动学树由两个C++对象表示，分别称为Model（包含模型常量：长度、质量、名称等）和Data（包含模型算法使用的工作内存）。这两个C++对象都包含在一个唯一的Python类中。第一个类称为RobotWrapper，它是通用的。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Nb joints = 7 (nq=6,nv=6)\n",
      "  Joint 0 universe: parent=0\n",
      "  Joint 1 shoulder_pan_joint: parent=0\n",
      "  Joint 2 shoulder_lift_joint: parent=1\n",
      "  Joint 3 elbow_joint: parent=2\n",
      "  Joint 4 wrist_1_joint: parent=3\n",
      "  Joint 5 wrist_2_joint: parent=4\n",
      "  Joint 6 wrist_3_joint: parent=5\n",
      "\n",
      "[0. 0. 0. 0. 0. 0.]\n"
     ]
    }
   ],
   "source": [
    "print(robot.model)\n",
    "print(robot.q0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For the next steps, we are going to work with the RobotWrapper.\n",
    "Import the class RobotWrapper and create an instance of this class in the python terminal. At initialization, RobotWrapper will read the model description in the URDF file given as argument. In the following, we will use the model of the UR5 robot, available in the directory \"models\" of pinocchio (available in the homedir of the VBox). The code of the RobotWrapper class is in /opt/openrobots/lib/python2.7/site-packages/pinocchio/robot_wrapper.py . Do not hesitate to have a look at it and to take inspiration from the implementation of the class functions.\n",
    "\n",
    "Here are some import methods of the class.\n",
    "\n",
    "导入`RobotWrapper`类，并在Python终端中创建该类的一个实例。在初始化时，`RobotWrapper`将读取作为参数给出的URDF文件中的模型描述。接下来，我们将使用UR5机器人的模型，该模型可在pinocchio的“models”目录中找到（可在虚拟机的主目录中找到）。`RobotWrapper`类的代码位于`/opt/openrobots/lib/python2.7/site-packages/pinocchio/robot_wrapper.py`。请随时查看该代码，并从类函数的实现中获取灵感。\n",
    "\n",
    "以下是该类的一些导入方法。\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* robot.q0 contains a reference initial configuration of the robot (not a pretty good one for the UR-5)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* robot.index('joint name') returns the index of the joint."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "0"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "robot.index(' wrist_3_joint')\n",
    "robot.index('universe')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* robot.model.names is a container (~list) that contains all the joint names"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "0 universe\n",
      "1 shoulder_pan_joint\n",
      "2 shoulder_lift_joint\n",
      "3 elbow_joint\n",
      "4 wrist_1_joint\n",
      "5 wrist_2_joint\n",
      "6 wrist_3_joint\n"
     ]
    }
   ],
   "source": [
    "for i, n in enumerate(robot.model.names):\n",
    "    print(i, n)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* robot.model.frames contains all the import frames attached to the robot. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "universe attached to joint # 0\n",
      "root_joint attached to joint # 0\n",
      "world attached to joint # 0\n",
      "world_joint attached to joint # 0\n",
      "base_link attached to joint # 0\n",
      "base_link-base_fixed_joint attached to joint # 0\n",
      "base attached to joint # 0\n",
      "shoulder_pan_joint attached to joint # 1\n",
      "shoulder_link attached to joint # 1\n",
      "shoulder_lift_joint attached to joint # 2\n",
      "upper_arm_link attached to joint # 2\n",
      "elbow_joint attached to joint # 3\n",
      "forearm_link attached to joint # 3\n",
      "wrist_1_joint attached to joint # 4\n",
      "wrist_1_link attached to joint # 4\n",
      "wrist_2_joint attached to joint # 5\n",
      "wrist_2_link attached to joint # 5\n",
      "wrist_3_joint attached to joint # 6\n",
      "wrist_3_link attached to joint # 6\n",
      "ee_fixed_joint attached to joint # 6\n",
      "ee_link attached to joint # 6\n",
      "wrist_3_link-tool0_fixed_joint attached to joint # 6\n",
      "tool0 attached to joint # 6\n"
     ]
    }
   ],
   "source": [
    "for f in robot.model.frames:\n",
    "    print(f.name, 'attached to joint #', f.parent)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "robot.placement(idx) and robot.framePlacement(idx) returns the placement (i.e. translation+rotation of the joint / frame in argument.\n",
    "\n",
    "robot.placement(idx) 和 robot.framePlacement(idx) 返回参数中关节/框架的位姿（即平移+旋转）。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([ 4.79541401e-23,  1.00000000e+00, -4.89663865e-12])"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "a = robot.placement(robot.q0, 6)  # Placement of the end effector joint.末端执行器关节的位置\n",
    "b = robot.framePlacement(robot.q0, 22)  # Placement of the end effector tip.末端执行器尖端的位置\n",
    "\n",
    "tool_axis = b.rotation[:, 2]  # Axis of the tool\n",
    "tool_axis"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The dimension of the configuration space (i.e. the number of joints) is given in:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(6, 6)"
      ]
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "NQ = robot.model.nq\n",
    "NV = robot.model.nv  # for this simple robot, NV == NQ\n",
    "NQ,NV"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Display simple geometries\n",
    "The robot is displayed in the viewer. We are going to use Meshcat to visualize the 3d robot and scene. First open the viewer and load the robot geometries.\n",
    "\n",
    "机器人显示在查看器中。我们将使用Meshcat来可视化三维机器人和场景。首先打开查看器并加载机器人的几何图形。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "You can open the visualizer by visiting the following URL:\n",
      "http://127.0.0.1:7000/static/\n"
     ]
    }
   ],
   "source": [
    "from utils.meshcat_viewer_wrapper import MeshcatVisualizer, colors  # noqa: E402\n",
    "\n",
    "viz = MeshcatVisualizer(robot)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "            <div style=\"height: 400px; width: 100%; overflow-x: auto; overflow-y: hidden; resize: both\">\n",
       "            <iframe src=\"http://127.0.0.1:7000/static/\" style=\"width: 100%; height: 100%; border: none\"></iframe>\n",
       "            </div>\n",
       "            "
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A configuration *q* can be displayed in the viewer:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 33,
   "metadata": {},
   "outputs": [],
   "source": [
    "q = np.array([-2., -1.5, 2.1, -.5, -.5, 0])\n",
    "\n",
    "viz.display(q)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Other geometries (cubes, spheres, etc) can be displayed as well."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Add a red box in the viewer\n",
    "ballID = \"world/ball\"\n",
    "radius = 0.1\n",
    "viz.addSphere(ballID, radius, colors.red)\n",
    "\n",
    "# Place the ball at the position ( 0.5, 0.1, 0.2 )\n",
    "# The viewer expect position and rotation, apppend the identity quaternion\n",
    "o_ball = np.array([0.8, 0.3, 0.5])\n",
    "q_ball = o_ball.tolist() +  [1, 0, 0, 0]\n",
    "viz.applyConfiguration(ballID, q_ball)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Pick and place 3D"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Inverse geometry in 3D for pick position...\n",
    "\n",
    "We will use an inverse geometry method to find a configuration of the robot so that the end effector touches the ball anywhere on the ball. We will use an unconstrained optimization method.\n",
    "\n",
    "We can use the  the scipy solver [used in the previous notebook](1_geometry_2d.ipynb#section_optim), compute a configuration q where the end effector reaches p.\n",
    "\n",
    "For that, implement a cost function that takes a configuration as argument and returns the squared distance between the end effector tip (frame $22$) and the sphere, accounting for the fact we want to touch the ball on the boundary in the natural direction.\n",
    "\n",
    "Due to the convention used in the robot description, the natural direction of the tip is the $e_z$ axis of the frame\n",
    "\n",
    "我们将使用逆几何方法来找到机器人的一种构型，使得末端执行器能够在球的任何位置接触到球。我们将使用一种无约束优化方法。\n",
    "\n",
    "我们可以使用[上一个笔记本中使用的](1_geometry_2d.ipynb#section_optim) `scipy` 求解器，计算出末端执行器到达点 $p$ 的构型 $q$。\n",
    "\n",
    "为此，实现一个代价函数，该函数以一种构型作为参数，并返回末端执行器尖端（坐标系 $22$）与球体之间的平方距离，同时考虑到我们希望沿自然方向在球的边界上接触球这一事实。\n",
    "\n",
    "由于在机器人描述中使用的约定，尖端的自然方向是坐标系的 $e_z$ 轴。 "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [],
   "source": [
    "m = robot.framePlacement(q, 22)  # SE(3) element frame of the tip\n",
    "p = m.translation  # Position of the tip\n",
    "ez = m.rotation[:, 2]  # Direction of the tip"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[-0.0725617 , -0.04154536,  0.99649826],\n",
       "       [ 0.99350839, -0.09077827,  0.06855932],\n",
       "       [ 0.08761207,  0.99500417,  0.04786269]])"
      ]
     },
     "execution_count": 26,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "m.rotation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([0.8, 0.3, 0.5])"
      ]
     },
     "execution_count": 27,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "target = np.array(o_ball)  # x,y,z\n",
    "target"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [],
   "source": [
    "q_touch = robot.q0  # REPLACE WITH INVERSE GEOMETRY PROCEDURE 用逆几何程序替换"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here is a solution if you need it"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 34,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Optimization terminated successfully.\n",
      "         Current function value: 0.000000\n",
      "         Iterations: 11\n",
      "         Function evaluations: 84\n",
      "         Gradient evaluations: 12\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "array([ 0.02565526, -0.44636677, -0.21361703, -0.19303277,  0.36593748,\n",
       "        0.        ])"
      ]
     },
     "execution_count": 34,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# %do_not_load tp2/generated/invgeom3d_1\n",
    "\n",
    "def cost(q):\n",
    "    '''Compute score from a configuration'''\n",
    "    m = robot.framePlacement(q, 22)\n",
    "    p = m.translation\n",
    "    offset = m.rotation[:, 2] * radius\n",
    "    return norm(p +  offset - target)**2\n",
    "\n",
    "\n",
    "def callback(q):\n",
    "    viz.display(q)\n",
    "    time.sleep(0.5)\n",
    "\n",
    "q_touch = fmin_bfgs(cost, robot.q0, callback=callback)\n",
    "q_touch"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 124,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Take care to explicitely mention copy when you want a copy of array.\n",
    "q = q_touch.copy()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### ...and simulation of movement"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "At the reference position you built, the end effector placement (the main frame, not the tip) can be obtained by calling:\n",
    "\n",
    "在您构建的参考位置处，可以通过调用以下方式获得末端执行器的位置（主框架，而非尖端）："
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 35,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([-0.04405117, -0.35854079,  0.19743621])"
      ]
     },
     "execution_count": 35,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "robot.placement(q, 6).translation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Only the translation part of the placement has been selected. The rotation is free.\n",
    "\n",
    "Now, choose any trajectory you want in the configuration space (it can be sinus-cosinus waves, polynomials, splines, straight lines). Make a for loop to display the robot at sampling positions along this trajectory. The function sleep can be used to slow down the loop.\n",
    "\n",
    "仅选择了放置的平移部分。旋转是自由的。\n",
    "\n",
    "现在，在配置空间中选择任何你想要的轨迹（可以是正弦 - 余弦波、多项式、样条曲线、直线）。使用for循环来显示沿此轨迹采样位置的机器人。可以使用sleep函数来减慢循环速度。"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "At each instant of your loop, recompute the position of the ball and display it so that it always \"sticks\" to the robot end effector, by modifying the template code below:\n",
    "\n",
    "在循环的每一瞬间，重新计算球的位置并显示，使其始终 “粘附” 在机器人末端执行器上，方法是修改以下模板代码："
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 40,
   "metadata": {},
   "outputs": [],
   "source": [
    "q = q_touch.copy()\n",
    "vq = np.array([1., 0, 0, 0.5, 0, 0])\n",
    "idx = 6\n",
    "\n",
    "oMend = robot.placement(q, idx)\n",
    "# TODO: Compute the placement of the ball in the end effector frame\n",
    "end_ball = np.zeros(3)  # Position of ball center express in the frame of the end effector\n",
    "\n",
    "for i in range(200):\n",
    "    # Chose new configuration of the robot\n",
    "    q += vq / 40\n",
    "    q[2] = 1.71 + math.sin(i * 0.05) / 2\n",
    "\n",
    "    # TODO: Compute the new placement of the ball\n",
    "    o_ball = o_ball\n",
    "\n",
    "    # Display new configuration for robot and ball\n",
    "    viz.applyConfiguration(ballID, o_ball.tolist() + [1, 0, 0, 0])\n",
    "    viz.display(q)\n",
    "    time.sleep(1e-2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The solution is below, should you need it."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 42,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %do_not_load tp2/generated/simple_pick_and_place_4\n",
    "\n",
    "q = q_touch.copy()\n",
    "vq = np.array([2., 0, 0, 4., 0, 0])\n",
    "idx = 6\n",
    "\n",
    "oMend = robot.placement(q, idx)\n",
    "o_end = oMend.translation  # Position of end-eff express in world frame\n",
    "o_ball = q_ball[:3]  # Position of ball express in world frame\n",
    "o_end_ball = o_ball - o_end  # Relative position of ball center wrt end effector position, express in world frame\n",
    "end_ball = oMend.rotation.T @ o_end_ball  # Position of ball wrt eff in local coordinate\n",
    "\n",
    "for i in range(200):\n",
    "    # Chose new configuration of the robot\n",
    "    q += vq / 40\n",
    "    q[2] = 1.71 + math.sin(i * 0.05) / 2\n",
    "\n",
    "    # Gets the new position of the ball\n",
    "    oMend = robot.placement(q, idx)\n",
    "    o_ball = oMend * end_ball  # Apply oMend to the relative placement of ball\n",
    "\n",
    "    # Display new configuration for robot and ball\n",
    "    viz.applyConfiguration(ballID, o_ball.tolist() + [1, 0, 0, 0])\n",
    "    viz.display(q)\n",
    "    time.sleep(1e-2)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Pick and place 6D\n",
    "\n",
    "Say now that the object is a rectangle and not a sphere. We will pick the object at a reference position with the rotation that is imposed, so that the end effector is aligned with one of the faces of the rectangle.\n",
    "\n",
    "Let first create the objects:\n",
    "\n",
    "假设现在物体是一个矩形而非球体。我们将在一个参考位置选取该物体，并施加规定的旋转，以使末端执行器与矩形的其中一个面保持对齐。\n",
    "\n",
    "首先创建这些物体："
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 45,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Add a red box in the viewer\n",
    "boxID = \"world/box\"\n",
    "try:\n",
    "    viz.delete(ballID)\n",
    "except:\n",
    "    pass\n",
    "viz.addBox(boxID, [0.1, 0.2, 0.1], colors.magenta)\n",
    "\n",
    "# Place the box at the position (0.5, 0.1, 0.2) with no rotation\n",
    "oMbox = pin.SE3(np.eye(3), np.array([0.5, 0.1, 0.2]))  # x,y,z\n",
    "\n",
    "viz.applyConfiguration(boxID, oMbox)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will redo the same two questions as before (inverse geometry and movement simulation), but now also choosing the orientation of the box.\n",
    "\n",
    "我们将像之前一样重新做这两个问题（逆向几何和运动模拟），但现在还要选择盒子的方向。"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Inverse geometry in 6D...\n",
    "6D means: translation and rotation. Change the previous cost function for a cost measuring the \"placement difference\" between the current placement of the tip `robot.framePlacement(q, 22)` and a reference placement `oMtarget` of a face of the rectangle.\n",
    "\n",
    "For that, you can use the $SE(3)$ log function to score the distance between two placements. The log returns a 6D velocity, represented by a class `Motion`, that must be transformed to a vector of $\\mathbb{R}^6$ from which you can take the norm.\n",
    "\n",
    "6D 意味着：平移和旋转。修改之前的成本函数，使其用于衡量末端执行器当前位姿 `robot.framePlacement(q, 22)` 与矩形某一面的参考位姿 `oMtarget` 之间的 “位姿差异”。\n",
    "\n",
    "为此，你可以使用 $SE(3)$ 对数函数来计算两个位姿之间的距离。该对数函数返回一个 6D 速度，由 `Motion` 类表示，必须将其转换为 $\\mathbb{R}^6$ 中的向量，然后取其范数。 "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "boxMtarget:  R =\n",
      "          1           0           0\n",
      "          0 6.12323e-17           1\n",
      "          0          -1 6.12323e-17\n",
      "  p =    0 -0.1    0\n",
      "\n"
     ]
    }
   ],
   "source": [
    "# Relative placement of the left facet with the z-axis orthogonal to the facet pointing inside the box\n",
    "boxMtarget = pin.SE3(pin.utils.rotate('x', -np.pi / 2), np.array([0., -0.1, 0.]))\n",
    "print(f\"boxMtarget:{boxMtarget}\")\n",
    "\n",
    "# Placement of the facet in the world\n",
    "oMtarget = oMbox * boxMtarget # 右乘表示在当前运动坐标下变换"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 48,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The robot finally reached effector placement at\n",
      "   R =\n",
      "          -1            0  9.79328e-12\n",
      "           0            1            0\n",
      "-9.79328e-12            0           -1\n",
      "  p =   0.81725   0.10915 -0.005491\n",
      "\n"
     ]
    }
   ],
   "source": [
    "# TODO: place the box in target\n",
    "\n",
    "qopt = robot.q0  # Replace with optimization procedure\n",
    "\n",
    "print('The robot finally reached effector placement at\\n', robot.placement(qopt, 6))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The solution if you need it"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 49,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "         Current function value: 0.000000\n",
      "         Iterations: 64\n",
      "         Function evaluations: 908\n",
      "         Gradient evaluations: 128\n",
      "The robot finally reached effector placement at\n",
      "   R =\n",
      "           1  6.79723e-09 -6.20885e-09\n",
      "-6.79723e-09            1  1.99062e-09\n",
      " 6.20885e-09 -1.99062e-09            1\n",
      "  p =     0.5 -0.0823     0.2\n",
      "\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/home/uwsl/miniconda3/envs/robotics_course/lib/python3.10/site-packages/scipy/optimize/_optimize.py:1313: OptimizeWarning: Desired error not necessarily achieved due to precision loss.\n",
      "  res = _minimize_bfgs(f, x0, args, fprime, callback=callback, **opts)\n"
     ]
    }
   ],
   "source": [
    "# %do_not_load tp2/generated/invgeom6d_1\n",
    "\n",
    "viz.applyConfiguration(boxID, oMbox)\n",
    "\n",
    "def cost(q):\n",
    "    '''Compute score from a configuration'''\n",
    "    oMtip = robot.framePlacement(q, 22)\n",
    "    # Align tip placement and facet placement\n",
    "    return norm(pin.log(oMtip.inverse() * oMtarget).vector)\n",
    "\n",
    "\n",
    "def callback(q):\n",
    "    viz.display(q)\n",
    "    time.sleep(0.5)\n",
    "\n",
    "\n",
    "qopt = fmin_bfgs(cost, robot.q0, callback=callback)\n",
    "\n",
    "print('The robot finally reached effector placement at\\n', robot.placement(qopt, 6))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now move the box following the motion."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 50,
   "metadata": {},
   "outputs": [],
   "source": [
    "q = qopt.copy()\n",
    "vq = np.array([2., 0, 0, 4., 0, 0])\n",
    "idx = 6\n",
    "\n",
    "oMend = robot.placement(q, idx)\n",
    "# TODO: Compute the placement of the box wrt the end effector frame\n",
    "endMbox = pin.SE3()  # Placement of the box wrt end effector\n",
    "\n",
    "for i in range(100):\n",
    "    # Chose new configuration of the robot\n",
    "    q += vq / 40\n",
    "    q[2] = 1.71 + math.sin(i * 0.05) / 2\n",
    "\n",
    "    # TODO: replace with good calculation\n",
    "    oMbox = oMbox\n",
    "\n",
    "    # Display new configuration for robot and box\n",
    "    viz.applyConfiguration(boxID, oMbox)\n",
    "    viz.display(q)\n",
    "    time.sleep(1e-2)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And the solution if you need it"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 126,
   "metadata": {},
   "outputs": [
    {
     "ename": "ValueError",
     "evalue": "wrong argument size: expected 6, got 19\nhint: The configuration vector is not of right size\n",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mValueError\u001b[0m                                Traceback (most recent call last)",
      "Cell \u001b[0;32mIn[126], line 7\u001b[0m\n\u001b[1;32m      4\u001b[0m vq \u001b[38;5;241m=\u001b[39m np\u001b[38;5;241m.\u001b[39marray([\u001b[38;5;241m2.\u001b[39m, \u001b[38;5;241m0\u001b[39m, \u001b[38;5;241m0\u001b[39m, \u001b[38;5;241m4.\u001b[39m, \u001b[38;5;241m0\u001b[39m, \u001b[38;5;241m0\u001b[39m])\n\u001b[1;32m      5\u001b[0m idx \u001b[38;5;241m=\u001b[39m \u001b[38;5;241m6\u001b[39m\n\u001b[0;32m----> 7\u001b[0m oMend \u001b[38;5;241m=\u001b[39m \u001b[43mrobot\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mplacement\u001b[49m\u001b[43m(\u001b[49m\u001b[43mq\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43midx\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m      8\u001b[0m endMbox \u001b[38;5;241m=\u001b[39m oMend\u001b[38;5;241m.\u001b[39minverse() \u001b[38;5;241m*\u001b[39m oMbox  \u001b[38;5;66;03m# Placement of the box wrt end effector\u001b[39;00m\n\u001b[1;32m     10\u001b[0m \u001b[38;5;28;01mfor\u001b[39;00m i \u001b[38;5;129;01min\u001b[39;00m \u001b[38;5;28mrange\u001b[39m(\u001b[38;5;241m100\u001b[39m):\n\u001b[1;32m     11\u001b[0m     \u001b[38;5;66;03m# Chose new configuration of the robot\u001b[39;00m\n",
      "File \u001b[0;32m~/miniconda3/envs/robotics_course/lib/python3.10/site-packages/pinocchio/robot_wrapper.py:107\u001b[0m, in \u001b[0;36mRobotWrapper.placement\u001b[0;34m(self, q, index, update_kinematics)\u001b[0m\n\u001b[1;32m    105\u001b[0m \u001b[38;5;28;01mdef\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21mplacement\u001b[39m(\u001b[38;5;28mself\u001b[39m, q, index, update_kinematics\u001b[38;5;241m=\u001b[39m\u001b[38;5;28;01mTrue\u001b[39;00m):\n\u001b[1;32m    106\u001b[0m     \u001b[38;5;28;01mif\u001b[39;00m update_kinematics:\n\u001b[0;32m--> 107\u001b[0m         \u001b[43mpin\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mforwardKinematics\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;28;43mself\u001b[39;49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mmodel\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;28;43mself\u001b[39;49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mdata\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mq\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m    108\u001b[0m     \u001b[38;5;28;01mreturn\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mdata\u001b[38;5;241m.\u001b[39moMi[index]\n",
      "\u001b[0;31mValueError\u001b[0m: wrong argument size: expected 6, got 19\nhint: The configuration vector is not of right size\n"
     ]
    }
   ],
   "source": [
    "# %do_not_load tp2/generated/simple_pick_and_place_7\n",
    "\n",
    "q = qopt.copy()\n",
    "vq = np.array([2., 0, 0, 4., 0, 0])\n",
    "idx = 6\n",
    "\n",
    "oMend = robot.placement(q, idx)\n",
    "endMbox = oMend.inverse() * oMbox  # Placement of the box wrt end effector\n",
    "\n",
    "for i in range(100):\n",
    "    # Chose new configuration of the robot\n",
    "    q += vq / 40\n",
    "    q[2] = 1.71 + math.sin(i * 0.05) / 2\n",
    "\n",
    "    # Gets the new position of the box\n",
    "    oMend = robot.placement(q, idx)\n",
    "    oMbox = oMend * endMbox\n",
    "\n",
    "    # Display new configuration for robot and box\n",
    "    viz.applyConfiguration(boxID, oMbox)\n",
    "    viz.display(q)\n",
    "    time.sleep(1e-2)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## On inverse geometry while taking collisions into account"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As we have seen in the first TP, we can use constrain optimization solver and create a constraint related to the distance between object being positive. Under the hood, it use the distance computation and finite difference pattern to estimate the gradient and use it in the optimization procedure. If it worked well with the UR5 constrained to a plan with 2 degree of freedom, in the general case those approach get stuck. Recent advances on perturbed optimization allows to have better gradient for the collision function and nicely use it in optimization routine. [random smooth collision gradient](https://hal.archives-ouvertes.fr/hal-03780482/file/Differentiable_collision_detection.pdf)\n",
    "\n",
    "正如我们在第一个实践项目（TP）中所看到的，我们可以使用约束优化求解器，并创建一个与物体间距离为正相关的约束条件。在底层，它使用距离计算和有限差分模式来估计梯度，并将其用于优化过程。如果在UR5被约束在具有2个自由度的平面上时该方法效果良好，那么在一般情况下，这些方法会陷入困境。扰动优化方面的最新进展使得能够为碰撞函数获得更好的梯度，并在优化例程中很好地加以利用。[随机平滑碰撞梯度](https://hal.archives-ouvertes.fr/hal-03780482/file/Differentiable_collision_detection.pdf)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Optimizing in the quaternion space\n",
    "\n",
    "Let's now work with a floating robot: the quadruped ANYmal. This robot has 12 joints, but Q-space of size 19 (robot.model.nq) and Q-tangent space of size 18 (robot.model.nv). This is because with need 7D vector to encode the robot placement in space, which indeed to only 6 DOF.\n",
    "\n",
    "现在让我们来研究一个浮动基座机器人：四足机器人ANYmal。这个机器人有12个关节，但其构型空间（Q - space）的大小为19（robot.model.nq），切空间（Q - tangent space）的大小为18（robot.model.nv）。这是因为我们需要一个7维向量来编码机器人在空间中的位姿，而实际上只有6个自由度。 \n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 51,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "You can open the visualizer by visiting the following URL:\n",
      "http://127.0.0.1:7001/static/\n"
     ]
    },
    {
     "data": {
      "text/html": [
       "\n",
       "            <div style=\"height: 400px; width: 100%; overflow-x: auto; overflow-y: hidden; resize: both\">\n",
       "            <iframe src=\"http://127.0.0.1:7001/static/\" style=\"width: 100%; height: 100%; border: none\"></iframe>\n",
       "            </div>\n",
       "            "
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "execution_count": 51,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "robot = robex.load('solo12')\n",
    "viz = MeshcatVisualizer(robot)\n",
    "viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 52,
   "metadata": {},
   "outputs": [],
   "source": [
    "viz.display(robot.q0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "Run the following code. Can you explain what just happened? Then correct it to have a proper optimization of ANYmal configuration.\n",
    "\n",
    "运行以下代码。你能解释一下刚刚发生了什么吗？然后对其进行修正，以对ANYmal配置进行适当优化。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 53,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Optimization terminated successfully.\n",
      "         Current function value: 0.000000\n",
      "         Iterations: 46\n",
      "         Function evaluations: 1060\n",
      "         Gradient evaluations: 53\n"
     ]
    }
   ],
   "source": [
    "robot.feetIndexes = [robot.model.getFrameId(frameName) for frameName in ['HR_FOOT', 'HL_FOOT', 'FR_FOOT', 'FL_FOOT']]\n",
    "\n",
    "# --- Add box to represent target\n",
    "colors = ['red', 'blue', 'green', 'magenta']\n",
    "for color in colors:\n",
    "    viz.addSphere(\"world/%s\" % color, .05, color)\n",
    "    viz.addSphere(\"world/%s_des\" % color, .05, color)\n",
    "\n",
    "#\n",
    "# OPTIM 6D #########################################################\n",
    "#\n",
    "\n",
    "targets = [\n",
    "    np.array([-0.7, -0.2, 1.2]),\n",
    "    np.array([-0.3, 0.5, 0.8]),\n",
    "    np.array([0.3, 0.1, -0.1]),\n",
    "    np.array([0.9, 0.9, 0.5])\n",
    "]\n",
    "for i in range(4):\n",
    "    targets[i][2] += 1\n",
    "\n",
    "\n",
    "def cost(q):\n",
    "    '''Compute score from a configuration'''\n",
    "    cost = 0.\n",
    "    for i in range(4):\n",
    "        p_i = robot.framePlacement(q, robot.feetIndexes[i]).translation\n",
    "        cost += norm(p_i - targets[i])**2\n",
    "    return cost\n",
    "\n",
    "\n",
    "def callback(q):\n",
    "    viz.applyConfiguration('world/box', Mtarget)\n",
    "\n",
    "    for i in range(4):\n",
    "        p_i = robot.framePlacement(q, robot.feetIndexes[i])\n",
    "        viz.applyConfiguration('world/%s' % colors[i], p_i)\n",
    "        viz.applyConfiguration('world/%s_des' % colors[i], list(targets[i]) + [1, 0, 0, 0])\n",
    "\n",
    "    viz.display(q)\n",
    "    time.sleep(1e-2)\n",
    "\n",
    "\n",
    "Mtarget = pin.SE3(pin.utils.rotate('x', 3.14 / 4), np.array([0.5, 0.1, 0.2]))  # x,y,z\n",
    "qopt = fmin_bfgs(cost, robot.q0, callback=callback)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Configuration of parallel robots\n",
    "A parallel robot is composed of several kinematic chains (called the robot legs) that are all attached to the same end effector. This imposes strict constraints in the configuration space of the robot: a configuration is valide iff all the legs meets the same end-effector placement. We consider here only the geometry aspect of parallel robots (additionnally, some joints are not actuated, which causes additional problems).\n",
    "\n",
    "并联机器人由若干条运动链（称为机器人腿）组成，这些运动链都连接到同一个末端执行器上。这对机器人的构型空间施加了严格的限制：一种构型是有效的，当且仅当所有的腿都满足相同的末端执行器位置。我们这里只考虑并联机器人的几何方面（此外，一些关节是无驱动的，这会导致额外的问题）。\n",
    "\n",
    "The kinematic structure of a paralel robot indeed induces loops in the joint connection graph. In Pinocchio, we can only represents (one of) the underlying kinematic tree. The loop constraints have to be handled separately. An example that loads 4 manipulator arms is given below.\n",
    "\n",
    "并联机器人的运动学结构确实会在关节连接图中引入回路。在Pinocchio中，我们只能表示（其中之一）底层运动学树。回路约束必须单独处理。下面给出了一个加载4个机械臂的示例。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 65,
   "metadata": {},
   "outputs": [],
   "source": [
    "from utils. load_ur5_parallel import load_ur5_parallel  # noqa: E402\n",
    "\n",
    "robot = load_ur5_parallel()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 66,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "You can open the visualizer by visiting the following URL:\n",
      "http://127.0.0.1:7005/static/\n"
     ]
    },
    {
     "data": {
      "text/html": [
       "\n",
       "            <div style=\"height: 400px; width: 100%; overflow-x: auto; overflow-y: hidden; resize: both\">\n",
       "            <iframe src=\"http://127.0.0.1:7005/static/\" style=\"width: 100%; height: 100%; border: none\"></iframe>\n",
       "            </div>\n",
       "            "
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "execution_count": 66,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "viz = MeshcatVisualizer(robot)\n",
    "viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 69,
   "metadata": {},
   "outputs": [],
   "source": [
    "viz.display(robot.q0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 71,
   "metadata": {},
   "outputs": [],
   "source": [
    "[w, h, d] = [0.5, 0.5, 0.005]\n",
    "color = [red, green, blue, transparency] = [1, 1, 0.78, .8]\n",
    "viz.addBox('world/robot0/toolplate', [w, h, d], color)\n",
    "Mtool = pin.SE3(pin.utils.rotate('z', 1.268), np.array([0, 0, .75]))\n",
    "viz.applyConfiguration('world/robot0/toolplate', Mtool)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The 4 legs of the robot are loaded in a single robot model. The 4 effector placements are computed by:\n",
    "\n",
    "机器人的4条腿加载在单个机器人模型中。4个末端执行器的位置通过以下方式计算："
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 73,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "  R =\n",
       "   -0.707107    -0.707107 -3.46245e-12\n",
       "    0.707107    -0.707107 -3.46236e-12\n",
       "-6.12323e-17 -4.89664e-12            1\n",
       "  p = 0.306122 0.160483 0.749342"
      ]
     },
     "execution_count": 73,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "effIdxs = [robot.model.getFrameId('tool0_#%d' % i) for i in range(4)]\n",
    "robot.framePlacement(robot.q0, effIdxs[0])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The loop constraints are that the relative placement of every leg end-effector must stay the same that in the initial configuration given as example in with the configuration *robot.q0* and the plate placement *Mtool*. To be valid, a configuration *q* must satisfy these 4 relative placement constraints.\n",
    "\n",
    "循环约束条件是，每个腿部末端执行器的相对位置必须与初始配置中保持一致，初始配置以 *robot.q0* 给出的配置和 *Mtool* 给出的平台放置位置为例。为使配置 *q* 有效，它必须满足这4个相对位置约束条件。\n",
    "\n",
    "Consider now that the orientation of the tool plate is given by the following quaternion, with the translation that you like (see [the notebook about rotations if you need more details](appendix1_quaternions.ipynb)): \n",
    "\n",
    "现在假设工具板的方向由以下四元数给出，平移部分可自行选择（如需更多细节，请参阅[关于旋转的笔记本](appendix1_quaternions.ipynb)）：\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 72,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 0.13978495 -0.8172043   0.55913978]\n",
      " [ 0.98924731  0.13978495 -0.04301075]\n",
      " [-0.04301075  0.55913978  0.82795699]]\n"
     ]
    }
   ],
   "source": [
    "quat = pin.Quaternion(0.7, 0.2, 0.2, 0.6).normalized()\n",
    "print(quat.matrix())"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "- **Apply the rotation defined using the previous quaternion on the plate**\n",
    "- **Find using the above optimization routines the configuration of each robot leg so that the loop constraints are all met** for the new orientation of the plate\n",
    "\n",
    "- **将使用先前四元数定义的旋转应用于平板**\n",
    "- **利用上述优化程序，针对平板的新方向，找出每个机器人腿部的配置，使所有回路约束都得到满足**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 75,
   "metadata": {},
   "outputs": [],
   "source": [
    "# YOUR CODE"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "robotics_course",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.17"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
